PD_HGtest.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 "PD_HGtest.h"
00020 
00021 #include <iostream>
00022 
00023 #define DOF (2)
00024 #define TIMESTEP 0.002
00025 
00026 #define WAIST_FILE "etc/root.dat"
00027 
00028 namespace {
00029   const bool CONTROLLER_BRIDGE_DEBUG = false;
00030 }
00031 
00032 
00033 // Module specification
00034 // <rtc-template block="module_spec">
00035 static const char* PD_HGtest_spec[] =
00036   {
00037     "implementation_id", "PD_HGtest",
00038     "type_name",         "PD_HGtest",
00039     "description",       "Sample PD component",
00040     "version",           "0.1",
00041     "vendor",            "AIST",
00042     "category",          "Generic",
00043     "activity_type",     "DataFlowComponent",
00044     "max_instance",      "10",
00045     "language",          "C++",
00046     "lang_type",         "compile",
00047     // Configuration variables
00048 
00049     ""
00050   };
00051 // </rtc-template>
00052 
00053 PD_HGtest::PD_HGtest(RTC::Manager* manager)
00054   : RTC::DataFlowComponentBase(manager),
00055     // <rtc-template block="initializer">
00056     m_torque0Out("torque0", m_torque0),
00057     m_torque1Out("torque1", m_torque1),
00058     m_root_transOut("root_trans", m_root_trans),
00059     m_root_velOut("root_vel", m_root_vel),
00060     m_root_accOut("root_acc", m_root_acc)
00061     
00062 {
00063   if( CONTROLLER_BRIDGE_DEBUG )
00064   {
00065     std::cout << "PD_HGtest::PD_HGtest" << std::endl;
00066   }
00067 
00068   // Registration: InPort/OutPort/Service
00069   // <rtc-template block="registration">
00070 
00071   // Set service provider to Ports
00072   
00073   // Set service consumers to Ports
00074   
00075   // Set CORBA Service Ports
00076   
00077   // </rtc-template>
00078 }
00079 
00080 PD_HGtest::~PD_HGtest()
00081 {
00082   closeFiles();
00083 }
00084 
00085 
00086 RTC::ReturnCode_t PD_HGtest::onInitialize()
00087 {
00088   // <rtc-template block="bind_config">
00089   // Bind variables and configuration variable
00090   if( CONTROLLER_BRIDGE_DEBUG )
00091   {
00092     std::cout << "onInitialize" << std::endl;
00093   }
00094 
00095   // Set InPort buffers
00096   
00097   // Set OutPort buffer
00098   addOutPort("torque0", m_torque0Out);
00099   addOutPort("torque1", m_torque1Out);
00100   addOutPort("root_trans", m_root_transOut);
00101   addOutPort("root_vel", m_root_velOut);
00102   addOutPort("root_acc", m_root_accOut);
00103 
00104   // </rtc-template>
00105 
00106   m_torque0.data.length(1);
00107   m_torque1.data.length(1);
00108   m_root_vel.data.length(6);
00109   m_root_acc.data.length(6);
00110 
00111   return RTC::RTC_OK;
00112 }
00113 
00114 
00115 
00116 /*
00117 RTC::ReturnCode_t PD_HGtest::onFinalize()
00118 {
00119   return RTC::RTC_OK;
00120 }
00121 */
00122 
00123 /*
00124 RTC::ReturnCode_t PD_HGtest::onStartup(RTC::UniqueId ec_id)
00125 {
00126   return RTC::RTC_OK;
00127 }
00128 */
00129 
00130 /*
00131 RTC::ReturnCode_t PD_HGtest::onShutdown(RTC::UniqueId ec_id)
00132 {
00133   return RTC::RTC_OK;
00134 }
00135 */
00136 
00137 RTC::ReturnCode_t PD_HGtest::onActivated(RTC::UniqueId ec_id)
00138 {
00139   std::cout << "on Activated" << std::endl;
00140   openFiles();
00141 
00142   return RTC::RTC_OK;
00143 }
00144 
00145 
00146 RTC::ReturnCode_t PD_HGtest::onDeactivated(RTC::UniqueId ec_id)
00147 {
00148   std::cout << "on Deactivated" << std::endl;
00149   closeFiles();
00150   return RTC::RTC_OK;
00151 }
00152 
00153 
00154 
00155 RTC::ReturnCode_t PD_HGtest::onExecute(RTC::UniqueId ec_id)
00156 {
00157   if( CONTROLLER_BRIDGE_DEBUG )
00158   {
00159     std::cout << "onExecute" << std::endl;
00160     std::string localStr;
00161     std::cin >> localStr; 
00162   }
00163 
00164   static double root_x_p, root_x_v, root_x_a;
00165   if(!waist.eof()){
00166     waist >> root_x_p;  //skip time
00167     waist >> root_x_a;
00168     waist >> root_x_v;
00169     waist >> root_x_p;
00170   }
00171 
00172   m_torque0.data[0] = 0.0;
00173   m_torque1.data[0] = 0.0;
00174   
00175   m_root_trans.data.position.x = root_x_p;
00176   m_root_trans.data.position.y = 0;
00177   m_root_trans.data.position.z = 1;
00178   m_root_trans.data.orientation.r = 0;
00179   m_root_trans.data.orientation.p = 0;
00180   m_root_trans.data.orientation.y = 0;
00181   for(int i=0; i<6; i++)
00182     m_root_vel.data[i] = 0.0;
00183   m_root_vel.data[0] = root_x_v;
00184   for(int i=0; i<6; i++)
00185     m_root_acc.data[i] = 0.0;
00186   m_root_acc.data[0] = root_x_a;
00187 
00188   m_torque0Out.write();
00189   m_torque1Out.write();
00190   m_root_transOut.write();
00191   m_root_velOut.write();
00192   m_root_accOut.write();
00193 
00194   return RTC::RTC_OK;
00195 }
00196 
00197 
00198 /*
00199   RTC::ReturnCode_t PD_HGtest::onAborting(RTC::UniqueId ec_id)
00200   {
00201   return RTC::RTC_OK;
00202   }
00203 */
00204 
00205 /*
00206   RTC::ReturnCode_t PD_HGtest::onError(RTC::UniqueId ec_id)
00207   {
00208   return RTC::RTC_OK;
00209   }
00210 */
00211 
00212 /*
00213   RTC::ReturnCode_t PD_HGtest::onReset(RTC::UniqueId ec_id)
00214   {
00215   return RTC::RTC_OK;
00216   }
00217 */
00218 
00219 /*
00220   RTC::ReturnCode_t PD_HGtest::onStateUpdate(RTC::UniqueId ec_id)
00221   {
00222   return RTC::RTC_OK;
00223   }
00224 */
00225 
00226 /*
00227   RTC::ReturnCode_t PD_HGtest::onRateChanged(RTC::UniqueId ec_id)
00228   {
00229   return RTC::RTC_OK;
00230   }
00231 */
00232 
00233 void PD_HGtest::openFiles()
00234 {
00235   waist.open(WAIST_FILE);
00236   if (!waist.is_open())
00237   {
00238     std::cerr << WAIST_FILE << " not opened" << std::endl;
00239   }
00240 }
00241 
00242 void PD_HGtest::closeFiles()
00243 {
00244   if(waist.is_open()){
00245     waist.close();
00246     waist.clear();
00247   }
00248 }
00249 
00250 
00251 extern "C"
00252 {
00253 
00254   DLL_EXPORT void PD_HGtestInit(RTC::Manager* manager)
00255   {
00256     coil::Properties profile(PD_HGtest_spec);
00257     manager->registerFactory(profile,
00258                              RTC::Create<PD_HGtest>,
00259                              RTC::Delete<PD_HGtest>);
00260   }
00261 
00262 };
00263 


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:18