SampleHG.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
00002 /*
00003  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00004  * All rights reserved. This program is made available under the terms of the
00005  * Eclipse Public License v1.0 which accompanies this distribution, and is
00006  * available at http://www.eclipse.org/legal/epl-v10.html
00007  * Contributors:
00008  * National Institute of Advanced Industrial Science and Technology (AIST)
00009  * General Robotix Inc. 
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 // Module specification
00033 // <rtc-template block="module_spec">
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     // Configuration variables
00047 
00048     ""
00049   };
00050 // </rtc-template>
00051 
00052 SampleHG::SampleHG(RTC::Manager* manager)
00053   : RTC::DataFlowComponentBase(manager),
00054     // <rtc-template block="initializer">
00055     m_angleOut("angle", m_angle),
00056     m_velOut("vel", m_vel),
00057     m_accOut("acc", m_acc)
00058     
00059     // </rtc-template>
00060 {
00061   if( CONTROLLER_BRIDGE_DEBUG )
00062   {
00063     std::cout << "SampleHG::SampleHG" << std::endl;
00064   }
00065   // Registration: InPort/OutPort/Service
00066   // <rtc-template block="registration">
00067 
00068   // Set service provider to Ports
00069   
00070   // Set service consumers to Ports
00071   
00072   // Set CORBA Service Ports
00073   
00074   // </rtc-template>
00075 }
00076 
00077 SampleHG::~SampleHG()
00078 {
00079     closeFiles();
00080 }
00081 
00082 
00083 RTC::ReturnCode_t SampleHG::onInitialize()
00084 {
00085   // <rtc-template block="bind_config">
00086   // Bind variables and configuration variable
00087   if( CONTROLLER_BRIDGE_DEBUG )
00088   {
00089     std::cout << "onInitialize" << std::endl;
00090   }
00091 
00092   // Set InPort buffers
00093 
00094   // Set OutPort buffer
00095   addOutPort("angle", m_angleOut);
00096   addOutPort("vel", m_velOut);
00097   addOutPort("acc", m_accOut);
00098   // </rtc-template>
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 RTC::ReturnCode_t SampleHG::onFinalize()
00111 {
00112   return RTC::RTC_OK;
00113 }
00114 */
00115 
00116 /*
00117 RTC::ReturnCode_t SampleHG::onStartup(RTC::UniqueId ec_id)
00118 {
00119   return RTC::RTC_OK;
00120 }
00121 */
00122 
00123 /*
00124 RTC::ReturnCode_t SampleHG::onShutdown(RTC::UniqueId ec_id)
00125 {
00126   return RTC::RTC_OK;
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; // skip time
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   RTC::ReturnCode_t SampleHG::onAborting(RTC::UniqueId ec_id)
00188   {
00189   return RTC::RTC_OK;
00190   }
00191 */
00192 
00193 /*
00194   RTC::ReturnCode_t SampleHG::onError(RTC::UniqueId ec_id)
00195   {
00196   return RTC::RTC_OK;
00197   }
00198 */
00199 
00200 /*
00201   RTC::ReturnCode_t SampleHG::onReset(RTC::UniqueId ec_id)
00202   {
00203   return RTC::RTC_OK;
00204   }
00205 */
00206 
00207 /*
00208   RTC::ReturnCode_t SampleHG::onStateUpdate(RTC::UniqueId ec_id)
00209   {
00210   return RTC::RTC_OK;
00211   }
00212 */
00213 
00214 /*
00215   RTC::ReturnCode_t SampleHG::onRateChanged(RTC::UniqueId ec_id)
00216   {
00217   return RTC::RTC_OK;
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 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19