Joystick2Velocity3D.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 "Joystick2Velocity3D.h"
00014 
00015 // Module specification
00016 // <rtc-template block="module_spec">
00017 static const char* joystick2velocity_spec[] =
00018   {
00019     "implementation_id", "Joystick2Velocity3D",
00020     "type_name",         "Joystick2Velocity3D",
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.scalesTranslation", "1.0,1.0,1.0",
00033     "conf.default.scalesRotation", "1.0,1.0,1.0",
00034     "conf.default.rotateModeButton", "9",
00035     ""
00036   };
00037 // </rtc-template>
00038 
00039 Joystick2Velocity3D::Joystick2Velocity3D(RTC::Manager* manager)
00040   : RTC::DataFlowComponentBase(manager),
00041     // <rtc-template block="initializer">
00042     m_axesIn("axes", m_axes),
00043     m_buttonsIn("buttons", m_buttons),
00044     m_velOut("vel", m_vel),
00045     m_mirroredVelOut("mirroredVel", m_mirroredVel),
00046     // </rtc-template>
00047     dummy(0),
00048     m_debugLevel(0)
00049 {
00050   // Registration: InPort/OutPort/Service
00051   // <rtc-template block="registration">
00052   // Set InPort buffers
00053 
00054   // Set OutPort buffer
00055   
00056   // Set service provider to Ports
00057   
00058   // Set service consumers to Ports
00059   
00060   // Set CORBA Service Ports
00061   
00062   // </rtc-template>
00063 }
00064 
00065 Joystick2Velocity3D::~Joystick2Velocity3D()
00066 {
00067 }
00068 
00069 
00070 
00071 RTC::ReturnCode_t Joystick2Velocity3D::onInitialize()
00072 {
00073   //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
00074   // <rtc-template block="bind_config">
00075   // Bind variables and configuration variable
00076   bindParameter("debugLevel", m_debugLevel, "0");
00077   bindParameter("axesIds", m_axesIds, "0,1,2");
00078   bindParameter("scalesTranslation", m_scalesTranslation, "1.0,1.0,1.0");
00079   bindParameter("scalesRotation", m_scalesRotation, "1.0,1.0,1.0");
00080   bindParameter("rotateModeButton", m_rotateModeButton, "9");
00081 
00082   // </rtc-template>
00083   addInPort("axes", m_axesIn);
00084   addInPort("buttons", m_buttonsIn);
00085   addOutPort("vel", m_velOut);
00086   addOutPort("mirroredVel", m_mirroredVelOut);
00087 
00088   m_vel.data.vx = m_vel.data.vy = m_vel.data.vz = 0;
00089   m_vel.data.vr = m_vel.data.vp = m_vel.data.va = 0;
00090   m_mirroredVel.data.vx = m_mirroredVel.data.vy = m_mirroredVel.data.vz = 0;
00091   m_mirroredVel.data.vr = m_mirroredVel.data.vp = m_mirroredVel.data.va = 0;
00092   m_axes.data.length(4);
00093   for (unsigned int i=0; i<m_axes.data.length(); i++){
00094       m_axes.data[i] = 0.0;
00095   }
00096 
00097   return RTC::RTC_OK;
00098 }
00099 
00100 
00101 
00102 /*
00103 RTC::ReturnCode_t Joystick2Velocity3D::onFinalize()
00104 {
00105   return RTC::RTC_OK;
00106 }
00107 */
00108 
00109 /*
00110 RTC::ReturnCode_t Joystick2Velocity3D::onStartup(RTC::UniqueId ec_id)
00111 {
00112   return RTC::RTC_OK;
00113 }
00114 */
00115 
00116 /*
00117 RTC::ReturnCode_t Joystick2Velocity3D::onShutdown(RTC::UniqueId ec_id)
00118 {
00119   return RTC::RTC_OK;
00120 }
00121 */
00122 
00123 RTC::ReturnCode_t Joystick2Velocity3D::onActivated(RTC::UniqueId ec_id)
00124 {
00125   std::cout << "Joystick2Velocity3D::onActivated(" << ec_id << ")" << std::endl;
00126   return RTC::RTC_OK;
00127 }
00128 
00129 RTC::ReturnCode_t Joystick2Velocity3D::onDeactivated(RTC::UniqueId ec_id)
00130 {
00131   std::cout << "Joystick2Velocity3D::onDeactivated(" << ec_id << ")" << std::endl;
00132   return RTC::RTC_OK;
00133 }
00134 
00135 RTC::ReturnCode_t Joystick2Velocity3D::onExecute(RTC::UniqueId ec_id)
00136 {
00137   if (m_debugLevel > 0){
00138       std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" 
00139                 << std::endl;
00140   }
00141 
00142   if (m_axesIn.isNew()) m_axesIn.read();
00143   if (m_buttonsIn.isNew()) m_buttonsIn.read();
00144   
00145   bool isPushed = false;
00146   for( unsigned int i = 0 ; i < m_buttons.data.length() ; i++ )
00147     isPushed |= m_buttons.data[i];
00148   
00149   if (m_buttons.data[m_rotateModeButton]){
00150       m_vel.data.vx = m_vel.data.vy = m_vel.data.vz = 0.0;
00151       m_vel.data.vr = -m_scalesRotation[1]*m_axes.data[m_axesIds[1]];
00152       m_vel.data.vp =  m_scalesRotation[0]*m_axes.data[m_axesIds[0]];
00153       m_vel.data.va =  m_scalesRotation[2]*m_axes.data[m_axesIds[2]];
00154       //
00155       m_mirroredVel.data.vx = m_mirroredVel.data.vy = m_mirroredVel.data.vz = 0.0;
00156       m_mirroredVel.data.vr = -m_vel.data.vr;
00157       m_mirroredVel.data.vp =  m_vel.data.vp;
00158       m_mirroredVel.data.va = -m_vel.data.va;
00159   }else if( !isPushed ){
00160       m_vel.data.vx = m_scalesTranslation[0]*m_axes.data[m_axesIds[0]];
00161       m_vel.data.vy = m_scalesTranslation[1]*m_axes.data[m_axesIds[1]];
00162       m_vel.data.vz = m_scalesTranslation[2]*m_axes.data[m_axesIds[2]];
00163       m_vel.data.vr = m_vel.data.vp = m_vel.data.va = 0.0;
00164       //
00165       m_mirroredVel.data.vx =  m_vel.data.vx;
00166       m_mirroredVel.data.vy = -m_vel.data.vy;
00167       m_mirroredVel.data.vz =  m_vel.data.vz;
00168       m_mirroredVel.data.vr = m_mirroredVel.data.vp = m_mirroredVel.data.va = 0.0;
00169   }
00170   if (m_debugLevel > 0) {
00171       printf("velocity command: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f\n", 
00172              m_vel.data.vx, m_vel.data.vy, m_vel.data.vz,
00173              m_vel.data.vr, m_vel.data.vp, m_vel.data.va);
00174   }
00175   m_velOut.write();
00176   m_mirroredVelOut.write();
00177 
00178   return RTC::RTC_OK;
00179 }
00180 
00181 /*
00182 RTC::ReturnCode_t Joystick2Velocity3D::onAborting(RTC::UniqueId ec_id)
00183 {
00184   return RTC::RTC_OK;
00185 }
00186 */
00187 
00188 /*
00189 RTC::ReturnCode_t Joystick2Velocity3D::onError(RTC::UniqueId ec_id)
00190 {
00191   return RTC::RTC_OK;
00192 }
00193 */
00194 
00195 /*
00196 RTC::ReturnCode_t Joystick2Velocity3D::onReset(RTC::UniqueId ec_id)
00197 {
00198   return RTC::RTC_OK;
00199 }
00200 */
00201 
00202 /*
00203 RTC::ReturnCode_t Joystick2Velocity3D::onStateUpdate(RTC::UniqueId ec_id)
00204 {
00205   return RTC::RTC_OK;
00206 }
00207 */
00208 
00209 /*
00210 RTC::ReturnCode_t Joystick2Velocity3D::onRateChanged(RTC::UniqueId ec_id)
00211 {
00212   return RTC::RTC_OK;
00213 }
00214 */
00215 
00216 
00217 
00218 extern "C"
00219 {
00220 
00221   void Joystick2Velocity3DInit(RTC::Manager* manager)
00222   {
00223     RTC::Properties profile(joystick2velocity_spec);
00224     manager->registerFactory(profile,
00225                              RTC::Create<Joystick2Velocity3D>,
00226                              RTC::Delete<Joystick2Velocity3D>);
00227   }
00228 
00229 };
00230 
00231 


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