SampleHG.cpp
Go to the documentation of this file.
1 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
2 /*
3  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
4  * All rights reserved. This program is made available under the terms of the
5  * Eclipse Public License v1.0 which accompanies this distribution, and is
6  * available at http://www.eclipse.org/legal/epl-v10.html
7  * Contributors:
8  * National Institute of Advanced Industrial Science and Technology (AIST)
9  * General Robotix Inc.
10  */
19 #include "SampleHG.h"
20 
21 #include <iostream>
22 
23 #define ANGLE_FILE "etc/angle.dat"
24 #define VEL_FILE "etc/vel.dat"
25 #define ACC_FILE "etc/acc.dat"
26 
27 namespace {
28  const bool CONTROLLER_BRIDGE_DEBUG = false;
29 }
30 
31 
32 // Module specification
33 // <rtc-template block="module_spec">
34 static const char* samplepd_spec[] =
35  {
36  "implementation_id", "SampleHG",
37  "type_name", "SampleHG",
38  "description", "Sample HG component",
39  "version", "0.1",
40  "vendor", "AIST",
41  "category", "Generic",
42  "activity_type", "DataFlowComponent",
43  "max_instance", "10",
44  "language", "C++",
45  "lang_type", "compile",
46  // Configuration variables
47 
48  ""
49  };
50 // </rtc-template>
51 
53  : RTC::DataFlowComponentBase(manager),
54  // <rtc-template block="initializer">
55  m_angleOut("angle", m_angle),
56  m_velOut("vel", m_vel),
57  m_accOut("acc", m_acc)
58 
59  // </rtc-template>
60 {
61  if( CONTROLLER_BRIDGE_DEBUG )
62  {
63  std::cout << "SampleHG::SampleHG" << std::endl;
64  }
65  // Registration: InPort/OutPort/Service
66  // <rtc-template block="registration">
67 
68  // Set service provider to Ports
69 
70  // Set service consumers to Ports
71 
72  // Set CORBA Service Ports
73 
74  // </rtc-template>
75 }
76 
78 {
79  closeFiles();
80 }
81 
82 
83 RTC::ReturnCode_t SampleHG::onInitialize()
84 {
85  // <rtc-template block="bind_config">
86  // Bind variables and configuration variable
87  if( CONTROLLER_BRIDGE_DEBUG )
88  {
89  std::cout << "onInitialize" << std::endl;
90  }
91 
92  // Set InPort buffers
93 
94  // Set OutPort buffer
95  addOutPort("angle", m_angleOut);
96  addOutPort("vel", m_velOut);
97  addOutPort("acc", m_accOut);
98  // </rtc-template>
99 
100  m_angle.data.length(DOF);
101  m_vel.data.length(DOF);
102  m_acc.data.length(DOF);
103 
104  return RTC::RTC_OK;
105 }
106 
107 
108 
109 /*
110 RTC::ReturnCode_t SampleHG::onFinalize()
111 {
112  return RTC::RTC_OK;
113 }
114 */
115 
116 /*
117 RTC::ReturnCode_t SampleHG::onStartup(RTC::UniqueId ec_id)
118 {
119  return RTC::RTC_OK;
120 }
121 */
122 
123 /*
124 RTC::ReturnCode_t SampleHG::onShutdown(RTC::UniqueId ec_id)
125 {
126  return RTC::RTC_OK;
127 }
128 */
129 
130 RTC::ReturnCode_t SampleHG::onActivated(RTC::UniqueId ec_id)
131 {
132 
133  std::cout << "on Activated" << std::endl;
134  openFiles();
135  for(int i = 0; i < DOF; ++i){
136  angle_ref[i] = vel_ref[i] = acc_ref[i] = 0.0;
137  }
138 
139  return RTC::RTC_OK;
140 }
141 
142 
143 RTC::ReturnCode_t SampleHG::onDeactivated(RTC::UniqueId ec_id)
144 {
145  std::cout << "on Deactivated" << std::endl;
146  closeFiles();
147  return RTC::RTC_OK;
148 }
149 
150 
151 RTC::ReturnCode_t SampleHG::onExecute(RTC::UniqueId ec_id)
152 {
153  if( CONTROLLER_BRIDGE_DEBUG )
154  {
155  std::cout << "SampleHG::onExecute" << std::endl;
156  }
157 
158  if(!angle.eof()){
159  double dummy;
160  angle >> dummy; vel >> dummy; acc >> dummy; // skip time
161 
162  int i;
163 
164  for (i=0; i<DOF; i++)
165  {
166  angle >> angle_ref[i];
167  vel >> vel_ref[i];
168  acc >> acc_ref[i];
169  }
170  }
171  for (int i=0; i<DOF; i++)
172  {
173  m_angle.data[i] = angle_ref[i];
174  m_vel.data[i] = vel_ref[i];
175  m_acc.data[i] = acc_ref[i];
176  }
177 
178  m_angleOut.write();
179  m_velOut.write();
180  m_accOut.write();
181 
182  return RTC::RTC_OK;
183 }
184 
185 
186 /*
187  RTC::ReturnCode_t SampleHG::onAborting(RTC::UniqueId ec_id)
188  {
189  return RTC::RTC_OK;
190  }
191 */
192 
193 /*
194  RTC::ReturnCode_t SampleHG::onError(RTC::UniqueId ec_id)
195  {
196  return RTC::RTC_OK;
197  }
198 */
199 
200 /*
201  RTC::ReturnCode_t SampleHG::onReset(RTC::UniqueId ec_id)
202  {
203  return RTC::RTC_OK;
204  }
205 */
206 
207 /*
208  RTC::ReturnCode_t SampleHG::onStateUpdate(RTC::UniqueId ec_id)
209  {
210  return RTC::RTC_OK;
211  }
212 */
213 
214 /*
215  RTC::ReturnCode_t SampleHG::onRateChanged(RTC::UniqueId ec_id)
216  {
217  return RTC::RTC_OK;
218  }
219 */
220 
222 {
223  angle.open(ANGLE_FILE);
224  if (!angle.is_open()){
225  std::cerr << ANGLE_FILE << " not opened" << std::endl;
226  }
227 
228  vel.open(VEL_FILE);
229  if (!vel.is_open()){
230  std::cerr << VEL_FILE << " not opened" << std::endl;
231  }
232 
233  acc.open(ACC_FILE);
234  if (!acc.is_open())
235  {
236  std::cerr << ACC_FILE << " not opend" << std::endl;
237  }
238 }
239 
241 {
242  if( angle.is_open() ){
243  angle.close();
244  angle.clear();
245  }
246  if( vel.is_open() ){
247  vel.close();
248  vel.clear();
249  }
250  if( acc.is_open() ){
251  acc.close();
252  acc.clear();
253  }
254 }
255 
256 
257 extern "C"
258 {
259 
261  {
263  manager->registerFactory(profile,
264  RTC::Create<SampleHG>,
265  RTC::Delete<SampleHG>);
266  }
267 
268 };
269 
png_infop png_charpp int png_charpp profile
Definition: png.h:2382
virtual RTC::ReturnCode_t onInitialize()
Definition: SampleHG.cpp:83
#define ACC_FILE
Definition: SampleHG.cpp:25
#define VEL_FILE
Definition: SampleHG.cpp:24
OutPort< TimedDoubleSeq > m_velOut
Definition: SampleHG.h:119
TimedDoubleSeq m_acc
Definition: SampleHG.h:121
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
Definition: SampleHG.cpp:143
std::ifstream angle
Definition: SampleHG.h:142
double angle_ref[DOF]
Definition: SampleHG.h:143
std::ifstream vel
Definition: SampleHG.h:142
TimedDoubleSeq m_angle
Definition: SampleHG.h:115
png_uint_32 i
Definition: png.h:2735
bool addOutPort(const char *name, OutPortBase &outport)
SampleHG(RTC::Manager *manager)
Definition: SampleHG.cpp:52
#define DOF
TimedDoubleSeq m_vel
Definition: SampleHG.h:118
DLL_EXPORT void SampleHGInit(RTC::Manager *manager)
Definition: SampleHG.cpp:260
double acc_ref[DOF]
Definition: SampleHG.h:143
OutPort< TimedDoubleSeq > m_accOut
Definition: SampleHG.h:122
ExecutionContextHandle_t UniqueId
void closeFiles()
Definition: SampleHG.cpp:240
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
Definition: SampleHG.cpp:130
#define ANGLE_FILE
Definition: SampleHG.cpp:23
double vel_ref[DOF]
Definition: SampleHG.h:143
virtual bool write(DataType &value)
std::ifstream acc
Definition: SampleHG.h:142
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
Definition: SampleHG.cpp:151
~SampleHG()
Definition: SampleHG.cpp:77
OutPort< TimedDoubleSeq > m_angleOut
Definition: SampleHG.h:116
bool registerFactory(coil::Properties &profile, RtcNewFunc new_func, RtcDeleteFunc delete_func)
void openFiles()
Definition: SampleHG.cpp:221
NewSample HG component.
static const char * samplepd_spec[]
Definition: SampleHG.cpp:34
#define DLL_EXPORT


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05