HiroStateHolder.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00007 #include "HiroStateHolder.h"
00008 #include <hrpUtil/Tvmet3d.h>
00009 
00010 
00011 // Module specification
00012 // <rtc-template block="module_spec">
00013 static const char* hirostateconverter_spec[] =
00014   {
00015     "implementation_id", "HiroStateHolder",
00016     "type_name",         "HiroStateHolder",
00017     "description",       "hiro - ros bridge",
00018     "version",           "1.0",
00019     "vendor",            "Kei Okada",
00020     "category",          "example",
00021     "activity_type",     "PERIODIC",
00022     "kind",              "DataFlowComponent",
00023     "max_instance",      "10",
00024     "language",          "C++",
00025     "lang_type",         "compile",
00026     // Configuration variables
00027     ""
00028   };
00029 // </rtc-template>
00030 
00031 HiroStateHolder::HiroStateHolder(RTC::Manager* manager)
00032     // <rtc-template block="initializer">
00033   : RTC::DataFlowComponentBase(manager),
00034     m_angleOut("angle", m_angle),
00035     m_rsangleOut("rsangle", m_rsangle),
00036     m_mcangleOut("mcangle", m_mcangle),
00037     m_rsrfsensorOut("rsrfsensor", m_rsrfsensor),
00038     m_rslfsensorOut("rslfsensor", m_rslfsensor),
00039     m_rsrhsensorOut("rsrhsensor", m_rsrhsensor),
00040     m_rslhsensorOut("rslhsensor", m_rslhsensor),
00041     m_gsensorOut("gsensor", m_gsensor),
00042     m_gyrometerOut("gyrometer", m_gyrometer),
00043     m_poseOut("pose", m_pose),
00044 
00045     m_jointDataIn("jointData", m_jointData),
00046     m_basePosIn("basePos", m_basePos),
00047     m_baseRpyIn("baseRpy", m_baseRpy)
00048     // </rtc-template>
00049 {
00050 }
00051 
00052 HiroStateHolder::~HiroStateHolder()
00053 {
00054 }
00055 
00056 
00057 RTC::ReturnCode_t HiroStateHolder::onInitialize()
00058 {
00059   // Registration: InPort/OutPort/Service
00060   // <rtc-template block="registration">
00061   // Set InPort buffers
00062   addInPort("jointData", m_jointDataIn);
00063   addInPort("basePos", m_basePosIn);
00064   addInPort("baseRpy", m_baseRpyIn);
00065 
00066   // Set OutPort buffer
00067   addOutPort("angle", m_angleOut);
00068   addOutPort("rsangle", m_rsangleOut);
00069   addOutPort("mcangle", m_mcangleOut);
00070   addOutPort("rsrfsensor", m_rsrfsensorOut);
00071   addOutPort("rslfsensor", m_rslfsensorOut);
00072   addOutPort("rsrhsensor", m_rsrhsensorOut);
00073   addOutPort("rslhsensor", m_rslhsensorOut);
00074   addOutPort("gsensor", m_gsensorOut);
00075   addOutPort("gyrometer", m_gyrometerOut);
00076   addOutPort("pose", m_poseOut);
00077 
00078   // Set service provider to Ports
00079 
00080   // Set service consumers to Ports
00081 
00082   // Set CORBA Service Ports
00083 
00084   // </rtc-template>
00085 
00086   // <rtc-template block="bind_config">
00087   // Bind variables and configuration variable
00088 
00089   // </rtc-template>
00090   return RTC::RTC_OK;
00091 }
00092 
00093 
00094 /*
00095 RTC::ReturnCode_t HiroStateHolder::onFinalize()
00096 {
00097   return RTC::RTC_OK;
00098 }
00099 */
00100 /*
00101 RTC::ReturnCode_t HiroStateHolder::onStartup(RTC::UniqueId ec_id)
00102 {
00103   return RTC::RTC_OK;
00104 }
00105 */
00106 /*
00107 RTC::ReturnCode_t HiroStateHolder::onShutdown(RTC::UniqueId ec_id)
00108 {
00109   return RTC::RTC_OK;
00110 }
00111 */
00112 /*
00113 RTC::ReturnCode_t HiroStateHolder::onActivated(RTC::UniqueId ec_id)
00114 {
00115   return RTC::RTC_OK;
00116 }
00117 */
00118 /*
00119 RTC::ReturnCode_t HiroStateHolder::onDeactivated(RTC::UniqueId ec_id)
00120 {
00121   return RTC::RTC_OK;
00122 }
00123 */
00124 
00125 RTC::ReturnCode_t HiroStateHolder::onExecute(RTC::UniqueId ec_id)
00126 {
00127   if ( m_jointDataIn.isNew() ) {
00128     m_jointDataIn.read();
00129     int k = 0;
00130     for (int i = 0; i < m_jointData.id.length(); i++ ) {
00131       k+= m_jointData.qCommand[i].length();
00132     }
00133     m_angle.data.length(k);
00134     m_rsangle.data.length(k);
00135     m_mcangle.data.length(k);
00136     k = 0;
00137     for (int i = 0; i < m_jointData.id.length(); i++ ) {
00138       for (int j = 0; j < m_jointData.qCommand[i].length(); j++ ) {
00139         m_angle.data[k]   = m_jointData.qState[i][j];
00140         m_mcangle.data[k] = m_jointData.qCommand[i][j];
00141         m_rsangle.data[k] = m_jointData.qState[i][j];
00142         k++;
00143       }
00144     }
00145 
00146     m_angleOut.write();
00147     m_mcangleOut.write();
00148     m_rsangleOut.write();
00149   }
00150 
00151   if ( m_basePosIn.isNew() && m_baseRpyIn.isNew() ) {
00152     m_basePosIn.read(); m_baseRpyIn.read();
00153     m_pose.data.position.x = m_basePos.data[0];
00154     m_pose.data.position.y = m_basePos.data[1];
00155     m_pose.data.position.z = m_basePos.data[2];
00156     hrp::Matrix33 R;
00157     hrp::getMatrix33FromRowMajorArray(R, m_baseRpy.data, 3);
00158     hrp::Vector3 rpy = hrp::rpyFromRot(R);
00159     m_pose.data.orientation.r = rpy[0];
00160     m_pose.data.orientation.p = rpy[1];
00161     m_pose.data.orientation.y = rpy[2];
00162     m_poseOut.write();
00163     }
00164   return RTC::RTC_OK;
00165 }
00166 
00167 /*
00168 RTC::ReturnCode_t HiroStateHolder::onAborting(RTC::UniqueId ec_id)
00169 {
00170   return RTC::RTC_OK;
00171 }
00172 */
00173 /*
00174 RTC::ReturnCode_t HiroStateHolder::onError(RTC::UniqueId ec_id)
00175 {
00176   return RTC::RTC_OK;
00177 }
00178 */
00179 /*
00180 RTC::ReturnCode_t HiroStateHolder::onReset(RTC::UniqueId ec_id)
00181 {
00182   return RTC::RTC_OK;
00183 }
00184 */
00185 /*
00186 RTC::ReturnCode_t HiroStateHolder::onStateUpdate(RTC::UniqueId ec_id)
00187 {
00188   return RTC::RTC_OK;
00189 }
00190 */
00191 /*
00192 RTC::ReturnCode_t HiroStateHolder::onRateChanged(RTC::UniqueId ec_id)
00193 {
00194   return RTC::RTC_OK;
00195 }
00196 */
00197 
00198 
00199 extern "C"
00200 {
00201  
00202   void HiroStateHolderInit(RTC::Manager* manager)
00203   {
00204     coil::Properties profile(hirostateconverter_spec);
00205     manager->registerFactory(profile,
00206                              RTC::Create<HiroStateHolder>,
00207                              RTC::Delete<HiroStateHolder>);
00208   }
00209   
00210 };
00211 
00212 
00213 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hironx_ros_bridge_old
Author(s): k-okada
autogenerated on Tue Jul 23 2013 11:49:38