Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00019 #include "SampleHG.h"
00020
00021 #include <iostream>
00022
00023 #define ANGLE_FILE "etc/angle.dat"
00024 #define VEL_FILE "etc/vel.dat"
00025 #define ACC_FILE "etc/acc.dat"
00026
00027 namespace {
00028 const bool CONTROLLER_BRIDGE_DEBUG = false;
00029 }
00030
00031
00032
00033
00034 static const char* samplepd_spec[] =
00035 {
00036 "implementation_id", "SampleHG",
00037 "type_name", "SampleHG",
00038 "description", "Sample HG component",
00039 "version", "0.1",
00040 "vendor", "AIST",
00041 "category", "Generic",
00042 "activity_type", "DataFlowComponent",
00043 "max_instance", "10",
00044 "language", "C++",
00045 "lang_type", "compile",
00046
00047
00048 ""
00049 };
00050
00051
00052 SampleHG::SampleHG(RTC::Manager* manager)
00053 : RTC::DataFlowComponentBase(manager),
00054
00055 m_angleOut("angle", m_angle),
00056 m_velOut("vel", m_vel),
00057 m_accOut("acc", m_acc)
00058
00059
00060 {
00061 if( CONTROLLER_BRIDGE_DEBUG )
00062 {
00063 std::cout << "SampleHG::SampleHG" << std::endl;
00064 }
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075 }
00076
00077 SampleHG::~SampleHG()
00078 {
00079 closeFiles();
00080 }
00081
00082
00083 RTC::ReturnCode_t SampleHG::onInitialize()
00084 {
00085
00086
00087 if( CONTROLLER_BRIDGE_DEBUG )
00088 {
00089 std::cout << "onInitialize" << std::endl;
00090 }
00091
00092
00093
00094
00095 addOutPort("angle", m_angleOut);
00096 addOutPort("vel", m_velOut);
00097 addOutPort("acc", m_accOut);
00098
00099
00100 m_angle.data.length(DOF);
00101 m_vel.data.length(DOF);
00102 m_acc.data.length(DOF);
00103
00104 return RTC::RTC_OK;
00105 }
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130 RTC::ReturnCode_t SampleHG::onActivated(RTC::UniqueId ec_id)
00131 {
00132
00133 std::cout << "on Activated" << std::endl;
00134 openFiles();
00135 for(int i = 0; i < DOF; ++i){
00136 angle_ref[i] = vel_ref[i] = acc_ref[i] = 0.0;
00137 }
00138
00139 return RTC::RTC_OK;
00140 }
00141
00142
00143 RTC::ReturnCode_t SampleHG::onDeactivated(RTC::UniqueId ec_id)
00144 {
00145 std::cout << "on Deactivated" << std::endl;
00146 closeFiles();
00147 return RTC::RTC_OK;
00148 }
00149
00150
00151 RTC::ReturnCode_t SampleHG::onExecute(RTC::UniqueId ec_id)
00152 {
00153 if( CONTROLLER_BRIDGE_DEBUG )
00154 {
00155 std::cout << "SampleHG::onExecute" << std::endl;
00156 }
00157
00158 if(!angle.eof()){
00159 double dummy;
00160 angle >> dummy; vel >> dummy; acc >> dummy;
00161
00162 int i;
00163
00164 for (i=0; i<DOF; i++)
00165 {
00166 angle >> angle_ref[i];
00167 vel >> vel_ref[i];
00168 acc >> acc_ref[i];
00169 }
00170 }
00171 for (int i=0; i<DOF; i++)
00172 {
00173 m_angle.data[i] = angle_ref[i];
00174 m_vel.data[i] = vel_ref[i];
00175 m_acc.data[i] = acc_ref[i];
00176 }
00177
00178 m_angleOut.write();
00179 m_velOut.write();
00180 m_accOut.write();
00181
00182 return RTC::RTC_OK;
00183 }
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221 void SampleHG::openFiles()
00222 {
00223 angle.open(ANGLE_FILE);
00224 if (!angle.is_open()){
00225 std::cerr << ANGLE_FILE << " not opened" << std::endl;
00226 }
00227
00228 vel.open(VEL_FILE);
00229 if (!vel.is_open()){
00230 std::cerr << VEL_FILE << " not opened" << std::endl;
00231 }
00232
00233 acc.open(ACC_FILE);
00234 if (!acc.is_open())
00235 {
00236 std::cerr << ACC_FILE << " not opend" << std::endl;
00237 }
00238 }
00239
00240 void SampleHG::closeFiles()
00241 {
00242 if( angle.is_open() ){
00243 angle.close();
00244 angle.clear();
00245 }
00246 if( vel.is_open() ){
00247 vel.close();
00248 vel.clear();
00249 }
00250 if( acc.is_open() ){
00251 acc.close();
00252 acc.clear();
00253 }
00254 }
00255
00256
00257 extern "C"
00258 {
00259
00260 DLL_EXPORT void SampleHGInit(RTC::Manager* manager)
00261 {
00262 coil::Properties profile(samplepd_spec);
00263 manager->registerFactory(profile,
00264 RTC::Create<SampleHG>,
00265 RTC::Delete<SampleHG>);
00266 }
00267
00268 };
00269