JoystickImpl.java
Go to the documentation of this file.
1 // -*- Java -*-
10 package jp.go.aist.hrp.joystick.rtc;
11 
12 import org.eclipse.swt.graphics.Point;
13 
15 import jp.go.aist.rtm.RTC.DataFlowComponentBase;
16 import jp.go.aist.rtm.RTC.Manager;
17 import jp.go.aist.rtm.RTC.port.OutPort;
18 import jp.go.aist.rtm.RTC.util.DataRef;
19 import RTC.ReturnCode_t;
20 import RTC.TimedFloatSeq;
21 import RTC.Time;
22 
23 
29 public class JoystickImpl extends DataFlowComponentBase {
30 
35  public JoystickImpl(Manager manager) {
36  super(manager);
37  }
38 
48  @Override
49  protected ReturnCode_t onInitialize() {
50 
51  // <rtc-template block="initializer">
52  m_pos_val = new TimedFloatSeq(new Time(0,0), new float[2]);
53  m_pos = new DataRef<TimedFloatSeq>(m_pos_val);
55  m_vel_val = new TimedFloatSeq(new Time(0,0), new float[2]);
56  m_vel = new DataRef<TimedFloatSeq>(m_vel_val);
58  // </rtc-template>
59 
60  // Registration: InPort/OutPort/Service
61  // <rtc-template block="registration">
62  // Set InPort buffers
63 
64  // Set OutPort buffer
65  try {
66  registerOutPort(TimedFloatSeq.class, "pos", m_posOut);
67  registerOutPort(TimedFloatSeq.class, "vel", m_velOut);
68  } catch (Exception e) {
69  e.printStackTrace();
70  }
71 
72  // Set service provider to Ports
73 
74  // Set service consumers to Ports
75 
76  // Set CORBA Service Ports
77 
78  // </rtc-template>
79 
80  return ReturnCode_t.RTC_OK;
81  }
82 
83  /***
84  *
85  * The finalize action (on ALIVE->END transition)
86  * formaer rtc_exiting_entry()
87  *
88  * @return RTC::ReturnCode_t
89  *
90  *
91  */
92  @Override
93  protected ReturnCode_t onFinalize() {
94  System.out.println("Joystick Finalized");
95  return super.onFinalize();
96  }
97 
98  /***
99  *
100  * The startup action when ExecutionContext startup
101  * former rtc_starting_entry()
102  *
103  * @param ec_id target ExecutionContext Id
104  *
105  * @return RTC::ReturnCode_t
106  *
107  *
108  */
109  @Override
110  protected ReturnCode_t onStartup(int ec_id) {
111  System.out.println("Joystick Started");
112  return super.onStartup(ec_id);
113  }
114 
115  /***
116  *
117  * The shutdown action when ExecutionContext stop
118  * former rtc_stopping_entry()
119  *
120  * @param ec_id target ExecutionContext Id
121  *
122  * @return RTC::ReturnCode_t
123  *
124  *
125  */
126  @Override
127  protected ReturnCode_t onShutdown(int ec_id) {
128  System.out.println("Joystick Shutting down");
129  return super.onShutdown(ec_id);
130  }
131 
132  /***
133  *
134  * The activated action (Active state entry action)
135  * former rtc_active_entry()
136  *
137  * @param ec_id target ExecutionContext Id
138  *
139  * @return RTC::ReturnCode_t
140  *
141  *
142  */
143  @Override
144  protected ReturnCode_t onActivated(int ec_id) {
145  System.out.println("Joystick Activated");
146  return super.onActivated(ec_id);
147  }
148 
149  /***
150  *
151  * The deactivated action (Active state exit action)
152  * former rtc_active_exit()
153  *
154  * @param ec_id target ExecutionContext Id
155  *
156  * @return RTC::ReturnCode_t
157  *
158  *
159  */
160  @Override
161  protected ReturnCode_t onDeactivated(int ec_id) {
162  System.out.println("Joystick Deactivated");
163  return super.onDeactivated(ec_id);
164  }
165 
166  /***
167  *
168  * The execution action that is invoked periodically
169  * former rtc_active_do()
170  *
171  * @param ec_id target ExecutionContext Id
172  *
173  * @return RTC::ReturnCode_t
174  *
175  *
176  */
177  @Override
178  protected ReturnCode_t onExecute(int ec_id) {
179 
180  Point p = joystickView.getJoystickPosition();
181  float[] pos = {p.x,p.y};
182  float[] vel = this.convert(p);
183  m_pos_val.data = pos;
184  m_vel_val.data = vel;
185  m_posOut.write();
186  m_velOut.write();
187 
188 // System.out.println("("+p.x+", "+p.y+"), ("+vel[0]+", "+vel[1]+")");
189 
190  return ReturnCode_t.RTC_OK;
191  }
192 
193 
200  protected float[] convert(Point p) {
201  double k=1.0;
202  double _th = Math.atan2(p.y,p.x);
203  double _v = k * Math.hypot(p.x, p.y);
204  double _vl = _v * Math.cos(_th - (Math.PI/4.0));
205  double _vr = _v * Math.sin(_th - (Math.PI/4.0));
206  if(_vr==-0.0) _vr*=-1;
207  float[] v = {(float) _vl, (float) _vr};
208  return v;
209  }
210 
211  /***
212  *
213  * The aborting action when main logic error occurred.
214  * former rtc_aborting_entry()
215  *
216  * @param ec_id target ExecutionContext Id
217  *
218  * @return RTC::ReturnCode_t
219  *
220  *
221  */
222  @Override
223  public ReturnCode_t onAborting(int ec_id) {
224  System.out.println("Joystick Aborted");
225  return super.onAborting(ec_id);
226  }
227 
228  /***
229  *
230  * The error action in ERROR state
231  * former rtc_error_do()
232  *
233  * @param ec_id target ExecutionContext Id
234  *
235  * @return RTC::ReturnCode_t
236  *
237  *
238  */
239  @Override
240  public ReturnCode_t onError(int ec_id) {
241  System.out.println("Joystick Error : "+ec_id);
242  return super.onError(ec_id);
243  }
244 
245  /***
246  *
247  * The reset action that is invoked resetting
248  * This is same but different the former rtc_init_entry()
249  *
250  * @param ec_id target ExecutionContext Id
251  *
252  * @return RTC::ReturnCode_t
253  *
254  *
255  */
256  @Override
257  protected ReturnCode_t onReset(int ec_id) {
258  System.out.println("Joystick Reset");
259  return super.onReset(ec_id);
260  }
261 
262  /***
263  *
264  * The state update action that is invoked after onExecute() action
265  * no corresponding operation exists in OpenRTm-aist-0.2.0
266  *
267  * @param ec_id target ExecutionContext Id
268  *
269  * @return RTC::ReturnCode_t
270  *
271  *
272  */
273  @Override
274  protected ReturnCode_t onStateUpdate(int ec_id) {
275  return super.onStateUpdate(ec_id);
276  }
277 
278  /***
279  *
280  * The action that is invoked when execution context's rate is changed
281  * no corresponding operation exists in OpenRTm-aist-0.2.0
282  *
283  * @param ec_id target ExecutionContext Id
284  *
285  * @return RTC::ReturnCode_t
286  *
287  *
288  */
289  @Override
290  protected ReturnCode_t onRateChanged(int ec_id) {
291  return super.onRateChanged(ec_id);
292  }
293 
294  // DataInPort declaration
295  // <rtc-template block="inport_declare">
296 
297  // </rtc-template>
298 
299  // DataOutPort declaration
300  // <rtc-template block="outport_declare">
301 
302  protected TimedFloatSeq m_pos_val;
303  protected DataRef<TimedFloatSeq> m_pos;
310 
311  protected TimedFloatSeq m_vel_val;
312  protected DataRef<TimedFloatSeq> m_vel;
319 
320  // </rtc-template>
321 
322  // CORBA Port declaration
323  // <rtc-template block="corbaport_declare">
324 
325  // </rtc-template>
326 
327  // Service declaration
328  // <rtc-template block="service_declare">
329 
330  // </rtc-template>
331 
332  // Consumer declaration
333  // <rtc-template block="consumer_declare">
334 
335  // </rtc-template>
336 
337 
338 }
Sample component for MobileRobotCanvas component.
ReturnCode_t
JoystickImpl(Manager manager)
constructor
float x
Definition: IcePoint.h:524
virtual bool write(TimedFloatSeq &value)
org


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:23