Joystick2Velocity2D.cpp
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #include <rtm/idl/BasicDataType.hh>
00011 #include <rtm/idl/ExtendedDataTypes.hh>
00012 #include "hrpsys/util/VectorConvert.h"
00013 #include "Joystick2Velocity2D.h"
00014 
00015 // Module specification
00016 // <rtc-template block="module_spec">
00017 static const char* joystick2velocity_spec[] =
00018   {
00019     "implementation_id", "Joystick2Velocity2D",
00020     "type_name",         "Joystick2Velocity2D",
00021     "description",       "joystick output to velocity converter",
00022     "version",           HRPSYS_PACKAGE_VERSION,
00023     "vendor",            "AIST",
00024     "category",          "example",
00025     "activity_type",     "DataFlowComponent",
00026     "max_instance",      "10",
00027     "language",          "C++",
00028     "lang_type",         "compile",
00029     // Configuration variables
00030     "conf.default.debugLevel", "0",
00031     "conf.default.axesIds", "0,1,2",
00032     "conf.default.scales", "1.0,1.0,1.0",
00033     "conf.default.neutrals", "0.0,0.0,0.0",
00034     ""
00035   };
00036 // </rtc-template>
00037 
00038 Joystick2Velocity2D::Joystick2Velocity2D(RTC::Manager* manager)
00039   : RTC::DataFlowComponentBase(manager),
00040     // <rtc-template block="initializer">
00041     m_axesIn("axes", m_axes),
00042     m_velOut("vel", m_vel),
00043     // </rtc-template>
00044     dummy(0),
00045     m_debugLevel(0)
00046 {
00047   // Registration: InPort/OutPort/Service
00048   // <rtc-template block="registration">
00049   // Set InPort buffers
00050 
00051   // Set OutPort buffer
00052   
00053   // Set service provider to Ports
00054   
00055   // Set service consumers to Ports
00056   
00057   // Set CORBA Service Ports
00058   
00059   // </rtc-template>
00060 }
00061 
00062 Joystick2Velocity2D::~Joystick2Velocity2D()
00063 {
00064 }
00065 
00066 
00067 
00068 RTC::ReturnCode_t Joystick2Velocity2D::onInitialize()
00069 {
00070   //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00071   // <rtc-template block="bind_config">
00072   // Bind variables and configuration variable
00073   bindParameter("debugLevel", m_debugLevel, "0");
00074   bindParameter("axesIds", m_axesIds, "0,1,2");
00075   bindParameter("scales", m_scales, "1.0,1.0,1.0");
00076   bindParameter("neutrals", m_neutrals, "0.0,0.0,0.0");
00077 
00078   // </rtc-template>
00079   addInPort("axes", m_axesIn);
00080   addOutPort("vel", m_velOut);
00081 
00082   m_axes.data.length(4);
00083   for (unsigned int i=0; i<m_axes.data.length(); i++){
00084       m_axes.data[i] = 0.0;
00085   }
00086 
00087   return RTC::RTC_OK;
00088 }
00089 
00090 
00091 
00092 /*
00093 RTC::ReturnCode_t Joystick2Velocity2D::onFinalize()
00094 {
00095   return RTC::RTC_OK;
00096 }
00097 */
00098 
00099 /*
00100 RTC::ReturnCode_t Joystick2Velocity2D::onStartup(RTC::UniqueId ec_id)
00101 {
00102   return RTC::RTC_OK;
00103 }
00104 */
00105 
00106 /*
00107 RTC::ReturnCode_t Joystick2Velocity2D::onShutdown(RTC::UniqueId ec_id)
00108 {
00109   return RTC::RTC_OK;
00110 }
00111 */
00112 
00113 RTC::ReturnCode_t Joystick2Velocity2D::onActivated(RTC::UniqueId ec_id)
00114 {
00115   return RTC::RTC_OK;
00116 }
00117 
00118 RTC::ReturnCode_t Joystick2Velocity2D::onDeactivated(RTC::UniqueId ec_id)
00119 {
00120   return RTC::RTC_OK;
00121 }
00122 
00123 RTC::ReturnCode_t Joystick2Velocity2D::onExecute(RTC::UniqueId ec_id)
00124 {
00125   if (m_debugLevel > 0){
00126       std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" 
00127                 << std::endl;
00128   }
00129 
00130   if (m_axesIn.isNew()) m_axesIn.read();
00131 
00132   m_vel.data.vx = m_neutrals[0] + m_scales[0]*m_axes.data[m_axesIds[0]];
00133   m_vel.data.vy = m_neutrals[1] + m_scales[1]*m_axes.data[m_axesIds[1]];
00134   m_vel.data.va = m_neutrals[2] + m_scales[2]*m_axes.data[m_axesIds[2]];
00135   if (m_debugLevel > 0) {
00136       printf("velocity command: %5.2f %5.2f %5.2f", 
00137              m_vel.data.vx, m_vel.data.vy, m_vel.data.va);
00138   }
00139   m_velOut.write();
00140 
00141   return RTC::RTC_OK;
00142 }
00143 
00144 /*
00145 RTC::ReturnCode_t Joystick2Velocity2D::onAborting(RTC::UniqueId ec_id)
00146 {
00147   return RTC::RTC_OK;
00148 }
00149 */
00150 
00151 /*
00152 RTC::ReturnCode_t Joystick2Velocity2D::onError(RTC::UniqueId ec_id)
00153 {
00154   return RTC::RTC_OK;
00155 }
00156 */
00157 
00158 /*
00159 RTC::ReturnCode_t Joystick2Velocity2D::onReset(RTC::UniqueId ec_id)
00160 {
00161   return RTC::RTC_OK;
00162 }
00163 */
00164 
00165 /*
00166 RTC::ReturnCode_t Joystick2Velocity2D::onStateUpdate(RTC::UniqueId ec_id)
00167 {
00168   return RTC::RTC_OK;
00169 }
00170 */
00171 
00172 /*
00173 RTC::ReturnCode_t Joystick2Velocity2D::onRateChanged(RTC::UniqueId ec_id)
00174 {
00175   return RTC::RTC_OK;
00176 }
00177 */
00178 
00179 
00180 
00181 extern "C"
00182 {
00183 
00184   void Joystick2Velocity2DInit(RTC::Manager* manager)
00185   {
00186     RTC::Properties profile(joystick2velocity_spec);
00187     manager->registerFactory(profile,
00188                              RTC::Create<Joystick2Velocity2D>,
00189                              RTC::Delete<Joystick2Velocity2D>);
00190   }
00191 
00192 };
00193 
00194 


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55