SampleRH2.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 "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 // Module specification
00032 // <rtc-template block="module_spec">
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     // Configuration variables
00046 
00047     ""
00048   };
00049 // </rtc-template>
00050 
00051 SampleRH2::SampleRH2(RTC::Manager* manager)
00052   : RTC::DataFlowComponentBase(manager),
00053     // <rtc-template block="initializer">
00054     m_root_transOut("root_trans", m_root_trans)
00055     // </rtc-template>
00056 {
00057   if( CONTROLLER_BRIDGE_DEBUG )
00058   {
00059     std::cout << "SampleRH2::SampleRH2" << std::endl;
00060   }
00061   // Registration: InPort/OutPort/Service
00062   // <rtc-template block="registration">
00063 
00064   // Set service provider to Ports
00065   
00066   // Set service consumers to Ports
00067   
00068   // Set CORBA Service Ports
00069   
00070   // </rtc-template>
00071 }
00072 
00073 SampleRH2::~SampleRH2()
00074 {
00075     closeFiles();
00076 }
00077 
00078 
00079 RTC::ReturnCode_t SampleRH2::onInitialize()
00080 {
00081   // <rtc-template block="bind_config">
00082   // Bind variables and configuration variable
00083   if( CONTROLLER_BRIDGE_DEBUG )
00084   {
00085     std::cout << "onInitialize" << std::endl;
00086   }
00087 
00088   // Set InPort buffers
00089 
00090   // Set OutPort buffer
00091   addOutPort("root_trans", m_root_transOut);
00092   // </rtc-template>
00093 
00094   return RTC::RTC_OK;
00095 }
00096 
00097 
00098 
00099 /*
00100 RTC::ReturnCode_t SampleRH2::onFinalize()
00101 {
00102   return RTC::RTC_OK;
00103 }
00104 */
00105 
00106 /*
00107 RTC::ReturnCode_t SampleRH2::onStartup(RTC::UniqueId ec_id)
00108 {
00109   return RTC::RTC_OK;
00110 }
00111 */
00112 
00113 /*
00114 RTC::ReturnCode_t SampleRH2::onShutdown(RTC::UniqueId ec_id)
00115 {
00116   return RTC::RTC_OK;
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  // for(int i = 0; i < DOF; ++i){
00126  //   angle_ref[i] = vel_ref[i] = acc_ref[i] = 0.0;
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   RTC::ReturnCode_t SampleRH2::onAborting(RTC::UniqueId ec_id)
00173   {
00174   return RTC::RTC_OK;
00175   }
00176 */
00177 
00178 /*
00179   RTC::ReturnCode_t SampleRH2::onError(RTC::UniqueId ec_id)
00180   {
00181   return RTC::RTC_OK;
00182   }
00183 */
00184 
00185 /*
00186   RTC::ReturnCode_t SampleRH2::onReset(RTC::UniqueId ec_id)
00187   {
00188   return RTC::RTC_OK;
00189   }
00190 */
00191 
00192 /*
00193   RTC::ReturnCode_t SampleRH2::onStateUpdate(RTC::UniqueId ec_id)
00194   {
00195   return RTC::RTC_OK;
00196   }
00197 */
00198 
00199 /*
00200   RTC::ReturnCode_t SampleRH2::onRateChanged(RTC::UniqueId ec_id)
00201   {
00202   return RTC::RTC_OK;
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 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56