Joystick2Velocity3D.cpp
Go to the documentation of this file.
1 // -*- C++ -*-
10 #include <rtm/idl/BasicDataType.hh>
11 #include <rtm/idl/ExtendedDataTypes.hh>
12 #include "hrpsys/util/VectorConvert.h"
13 #include "Joystick2Velocity3D.h"
14 
15 // Module specification
16 // <rtc-template block="module_spec">
17 static const char* joystick2velocity_spec[] =
18  {
19  "implementation_id", "Joystick2Velocity3D",
20  "type_name", "Joystick2Velocity3D",
21  "description", "joystick output to velocity converter",
22  "version", HRPSYS_PACKAGE_VERSION,
23  "vendor", "AIST",
24  "category", "example",
25  "activity_type", "DataFlowComponent",
26  "max_instance", "10",
27  "language", "C++",
28  "lang_type", "compile",
29  // Configuration variables
30  "conf.default.debugLevel", "0",
31  "conf.default.axesIds", "0,1,2",
32  "conf.default.scalesTranslation", "1.0,1.0,1.0",
33  "conf.default.scalesRotation", "1.0,1.0,1.0",
34  "conf.default.rotateModeButton", "9",
35  ""
36  };
37 // </rtc-template>
38 
40  : RTC::DataFlowComponentBase(manager),
41  // <rtc-template block="initializer">
42  m_axesIn("axes", m_axes),
43  m_buttonsIn("buttons", m_buttons),
44  m_velOut("vel", m_vel),
45  m_mirroredVelOut("mirroredVel", m_mirroredVel),
46  // </rtc-template>
47  dummy(0),
48  m_debugLevel(0)
49 {
50  // Registration: InPort/OutPort/Service
51  // <rtc-template block="registration">
52  // Set InPort buffers
53 
54  // Set OutPort buffer
55 
56  // Set service provider to Ports
57 
58  // Set service consumers to Ports
59 
60  // Set CORBA Service Ports
61 
62  // </rtc-template>
63 }
64 
66 {
67 }
68 
69 
70 
72 {
73  //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
74  // <rtc-template block="bind_config">
75  // Bind variables and configuration variable
76  bindParameter("debugLevel", m_debugLevel, "0");
77  bindParameter("axesIds", m_axesIds, "0,1,2");
78  bindParameter("scalesTranslation", m_scalesTranslation, "1.0,1.0,1.0");
79  bindParameter("scalesRotation", m_scalesRotation, "1.0,1.0,1.0");
80  bindParameter("rotateModeButton", m_rotateModeButton, "9");
81 
82  // </rtc-template>
83  addInPort("axes", m_axesIn);
84  addInPort("buttons", m_buttonsIn);
85  addOutPort("vel", m_velOut);
86  addOutPort("mirroredVel", m_mirroredVelOut);
87 
88  m_vel.data.vx = m_vel.data.vy = m_vel.data.vz = 0;
89  m_vel.data.vr = m_vel.data.vp = m_vel.data.va = 0;
90  m_mirroredVel.data.vx = m_mirroredVel.data.vy = m_mirroredVel.data.vz = 0;
91  m_mirroredVel.data.vr = m_mirroredVel.data.vp = m_mirroredVel.data.va = 0;
92  m_axes.data.length(4);
93  for (unsigned int i=0; i<m_axes.data.length(); i++){
94  m_axes.data[i] = 0.0;
95  }
96 
97  return RTC::RTC_OK;
98 }
99 
100 
101 
102 /*
103 RTC::ReturnCode_t Joystick2Velocity3D::onFinalize()
104 {
105  return RTC::RTC_OK;
106 }
107 */
108 
109 /*
110 RTC::ReturnCode_t Joystick2Velocity3D::onStartup(RTC::UniqueId ec_id)
111 {
112  return RTC::RTC_OK;
113 }
114 */
115 
116 /*
117 RTC::ReturnCode_t Joystick2Velocity3D::onShutdown(RTC::UniqueId ec_id)
118 {
119  return RTC::RTC_OK;
120 }
121 */
122 
124 {
125  std::cout << "Joystick2Velocity3D::onActivated(" << ec_id << ")" << std::endl;
126  return RTC::RTC_OK;
127 }
128 
130 {
131  std::cout << "Joystick2Velocity3D::onDeactivated(" << ec_id << ")" << std::endl;
132  return RTC::RTC_OK;
133 }
134 
136 {
137  if (m_debugLevel > 0){
138  std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")"
139  << std::endl;
140  }
141 
142  if (m_axesIn.isNew()) m_axesIn.read();
144 
145  bool isPushed = false;
146  for( unsigned int i = 0 ; i < m_buttons.data.length() ; i++ )
147  isPushed |= m_buttons.data[i];
148 
149  if (m_buttons.data[m_rotateModeButton]){
150  m_vel.data.vx = m_vel.data.vy = m_vel.data.vz = 0.0;
151  m_vel.data.vr = -m_scalesRotation[1]*m_axes.data[m_axesIds[1]];
152  m_vel.data.vp = m_scalesRotation[0]*m_axes.data[m_axesIds[0]];
153  m_vel.data.va = m_scalesRotation[2]*m_axes.data[m_axesIds[2]];
154  //
155  m_mirroredVel.data.vx = m_mirroredVel.data.vy = m_mirroredVel.data.vz = 0.0;
156  m_mirroredVel.data.vr = -m_vel.data.vr;
157  m_mirroredVel.data.vp = m_vel.data.vp;
158  m_mirroredVel.data.va = -m_vel.data.va;
159  }else if( !isPushed ){
160  m_vel.data.vx = m_scalesTranslation[0]*m_axes.data[m_axesIds[0]];
161  m_vel.data.vy = m_scalesTranslation[1]*m_axes.data[m_axesIds[1]];
162  m_vel.data.vz = m_scalesTranslation[2]*m_axes.data[m_axesIds[2]];
163  m_vel.data.vr = m_vel.data.vp = m_vel.data.va = 0.0;
164  //
165  m_mirroredVel.data.vx = m_vel.data.vx;
166  m_mirroredVel.data.vy = -m_vel.data.vy;
167  m_mirroredVel.data.vz = m_vel.data.vz;
168  m_mirroredVel.data.vr = m_mirroredVel.data.vp = m_mirroredVel.data.va = 0.0;
169  }
170  if (m_debugLevel > 0) {
171  printf("velocity command: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f\n",
172  m_vel.data.vx, m_vel.data.vy, m_vel.data.vz,
173  m_vel.data.vr, m_vel.data.vp, m_vel.data.va);
174  }
175  m_velOut.write();
177 
178  return RTC::RTC_OK;
179 }
180 
181 /*
182 RTC::ReturnCode_t Joystick2Velocity3D::onAborting(RTC::UniqueId ec_id)
183 {
184  return RTC::RTC_OK;
185 }
186 */
187 
188 /*
189 RTC::ReturnCode_t Joystick2Velocity3D::onError(RTC::UniqueId ec_id)
190 {
191  return RTC::RTC_OK;
192 }
193 */
194 
195 /*
196 RTC::ReturnCode_t Joystick2Velocity3D::onReset(RTC::UniqueId ec_id)
197 {
198  return RTC::RTC_OK;
199 }
200 */
201 
202 /*
203 RTC::ReturnCode_t Joystick2Velocity3D::onStateUpdate(RTC::UniqueId ec_id)
204 {
205  return RTC::RTC_OK;
206 }
207 */
208 
209 /*
210 RTC::ReturnCode_t Joystick2Velocity3D::onRateChanged(RTC::UniqueId ec_id)
211 {
212  return RTC::RTC_OK;
213 }
214 */
215 
216 
217 
218 extern "C"
219 {
220 
222  {
224  manager->registerFactory(profile,
225  RTC::Create<Joystick2Velocity3D>,
226  RTC::Delete<Joystick2Velocity3D>);
227  }
228 
229 };
230 
231 
ComponentProfile m_profile
png_infop png_charpp int png_charpp profile
InPort< TimedFloatSeq > m_axesIn
virtual RTC::ReturnCode_t onInitialize()
~Joystick2Velocity3D()
Destructor.
png_uint_32 i
bool addOutPort(const char *name, OutPortBase &outport)
OutPort< TimedVelocity3D > m_velOut
ExecutionContextHandle_t UniqueId
Joystick2Velocity3D(RTC::Manager *manager)
Constructor.
bool bindParameter(const char *param_name, VarType &var, const char *def_val, bool(*trans)(VarType &, const char *)=coil::stringTo)
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
OutPort< TimedVelocity3D > m_mirroredVelOut
std::vector< double > m_scalesRotation
joystick out to velocity converter
void Joystick2Velocity3DInit(RTC::Manager *manager)
std::vector< double > m_scalesTranslation
virtual bool isNew()
virtual bool write(DataType &value)
TimedBooleanSeq m_buttons
TimedVelocity3D m_mirroredVel
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
InPort< TimedBooleanSeq > m_buttonsIn
bool addInPort(const char *name, InPortBase &inport)
std::vector< unsigned int > m_axesIds
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
static const char * joystick2velocity_spec[]


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20