PD_HGtest.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 "PD_HGtest.h"
20 
21 #include <iostream>
22 
23 #define DOF (2)
24 #define TIMESTEP 0.002
25 
26 #define WAIST_FILE "etc/root.dat"
27 
28 namespace {
29  const bool CONTROLLER_BRIDGE_DEBUG = false;
30 }
31 
32 
33 // Module specification
34 // <rtc-template block="module_spec">
35 static const char* PD_HGtest_spec[] =
36  {
37  "implementation_id", "PD_HGtest",
38  "type_name", "PD_HGtest",
39  "description", "Sample PD component",
40  "version", "0.1",
41  "vendor", "AIST",
42  "category", "Generic",
43  "activity_type", "DataFlowComponent",
44  "max_instance", "10",
45  "language", "C++",
46  "lang_type", "compile",
47  // Configuration variables
48 
49  ""
50  };
51 // </rtc-template>
52 
53 PD_HGtest::PD_HGtest(RTC::Manager* manager)
54  : RTC::DataFlowComponentBase(manager),
55  // <rtc-template block="initializer">
56  m_torque0Out("torque0", m_torque0),
57  m_torque1Out("torque1", m_torque1),
58  m_root_transOut("root_trans", m_root_trans),
59  m_root_velOut("root_vel", m_root_vel),
60  m_root_accOut("root_acc", m_root_acc)
61 
62 {
63  if( CONTROLLER_BRIDGE_DEBUG )
64  {
65  std::cout << "PD_HGtest::PD_HGtest" << std::endl;
66  }
67 
68  // Registration: InPort/OutPort/Service
69  // <rtc-template block="registration">
70 
71  // Set service provider to Ports
72 
73  // Set service consumers to Ports
74 
75  // Set CORBA Service Ports
76 
77  // </rtc-template>
78 }
79 
81 {
82  closeFiles();
83 }
84 
85 
86 RTC::ReturnCode_t PD_HGtest::onInitialize()
87 {
88  // <rtc-template block="bind_config">
89  // Bind variables and configuration variable
90  if( CONTROLLER_BRIDGE_DEBUG )
91  {
92  std::cout << "onInitialize" << std::endl;
93  }
94 
95  // Set InPort buffers
96 
97  // Set OutPort buffer
98  addOutPort("torque0", m_torque0Out);
99  addOutPort("torque1", m_torque1Out);
100  addOutPort("root_trans", m_root_transOut);
101  addOutPort("root_vel", m_root_velOut);
102  addOutPort("root_acc", m_root_accOut);
103 
104  // </rtc-template>
105 
106  m_torque0.data.length(1);
107  m_torque1.data.length(1);
108  m_root_vel.data.length(6);
109  m_root_acc.data.length(6);
110 
111  return RTC::RTC_OK;
112 }
113 
114 
115 
116 /*
117 RTC::ReturnCode_t PD_HGtest::onFinalize()
118 {
119  return RTC::RTC_OK;
120 }
121 */
122 
123 /*
124 RTC::ReturnCode_t PD_HGtest::onStartup(RTC::UniqueId ec_id)
125 {
126  return RTC::RTC_OK;
127 }
128 */
129 
130 /*
131 RTC::ReturnCode_t PD_HGtest::onShutdown(RTC::UniqueId ec_id)
132 {
133  return RTC::RTC_OK;
134 }
135 */
136 
137 RTC::ReturnCode_t PD_HGtest::onActivated(RTC::UniqueId ec_id)
138 {
139  std::cout << "on Activated" << std::endl;
140  openFiles();
141 
142  return RTC::RTC_OK;
143 }
144 
145 
146 RTC::ReturnCode_t PD_HGtest::onDeactivated(RTC::UniqueId ec_id)
147 {
148  std::cout << "on Deactivated" << std::endl;
149  closeFiles();
150  return RTC::RTC_OK;
151 }
152 
153 
154 
155 RTC::ReturnCode_t PD_HGtest::onExecute(RTC::UniqueId ec_id)
156 {
157  if( CONTROLLER_BRIDGE_DEBUG )
158  {
159  std::cout << "onExecute" << std::endl;
160  std::string localStr;
161  std::cin >> localStr;
162  }
163 
164  static double root_x_p, root_x_v, root_x_a;
165  if(!waist.eof()){
166  waist >> root_x_p; //skip time
167  waist >> root_x_a;
168  waist >> root_x_v;
169  waist >> root_x_p;
170  }
171 
172  m_torque0.data[0] = 0.0;
173  m_torque1.data[0] = 0.0;
174 
175  m_root_trans.data.position.x = root_x_p;
176  m_root_trans.data.position.y = 0;
177  m_root_trans.data.position.z = 1;
178  m_root_trans.data.orientation.r = 0;
179  m_root_trans.data.orientation.p = 0;
180  m_root_trans.data.orientation.y = 0;
181  for(int i=0; i<6; i++)
182  m_root_vel.data[i] = 0.0;
183  m_root_vel.data[0] = root_x_v;
184  for(int i=0; i<6; i++)
185  m_root_acc.data[i] = 0.0;
186  m_root_acc.data[0] = root_x_a;
187 
188  m_torque0Out.write();
189  m_torque1Out.write();
190  m_root_transOut.write();
191  m_root_velOut.write();
192  m_root_accOut.write();
193 
194  return RTC::RTC_OK;
195 }
196 
197 
198 /*
199  RTC::ReturnCode_t PD_HGtest::onAborting(RTC::UniqueId ec_id)
200  {
201  return RTC::RTC_OK;
202  }
203 */
204 
205 /*
206  RTC::ReturnCode_t PD_HGtest::onError(RTC::UniqueId ec_id)
207  {
208  return RTC::RTC_OK;
209  }
210 */
211 
212 /*
213  RTC::ReturnCode_t PD_HGtest::onReset(RTC::UniqueId ec_id)
214  {
215  return RTC::RTC_OK;
216  }
217 */
218 
219 /*
220  RTC::ReturnCode_t PD_HGtest::onStateUpdate(RTC::UniqueId ec_id)
221  {
222  return RTC::RTC_OK;
223  }
224 */
225 
226 /*
227  RTC::ReturnCode_t PD_HGtest::onRateChanged(RTC::UniqueId ec_id)
228  {
229  return RTC::RTC_OK;
230  }
231 */
232 
234 {
235  waist.open(WAIST_FILE);
236  if (!waist.is_open())
237  {
238  std::cerr << WAIST_FILE << " not opened" << std::endl;
239  }
240 }
241 
243 {
244  if(waist.is_open()){
245  waist.close();
246  waist.clear();
247  }
248 }
249 
250 
251 extern "C"
252 {
253 
254  DLL_EXPORT void PD_HGtestInit(RTC::Manager* manager)
255  {
256  coil::Properties profile(PD_HGtest_spec);
257  manager->registerFactory(profile,
258  RTC::Create<PD_HGtest>,
259  RTC::Delete<PD_HGtest>);
260  }
261 
262 };
263 
PD_HGtest::m_torque0Out
OutPort< TimedDoubleSeq > m_torque0Out
Definition: PD_HGtest.h:115
PD_HGtest::~PD_HGtest
~PD_HGtest()
Definition: PD_HGtest.cpp:80
PD_HGtest::m_root_velOut
OutPort< TimedDoubleSeq > m_root_velOut
Definition: PD_HGtest.h:122
PD_HGtest::m_root_vel
TimedDoubleSeq m_root_vel
Definition: PD_HGtest.h:121
i
png_uint_32 i
Definition: png.h:2732
PD_HGtest::openFiles
void openFiles()
Definition: PD_HGtest.cpp:233
PD_HGtest.h
Sample PD component.
PD_HGtest::onActivated
virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id)
Definition: PD_HGtest.cpp:137
PD_HGtest::onExecute
virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id)
Definition: PD_HGtest.cpp:155
PD_HGtestInit
DLL_EXPORT void PD_HGtestInit(RTC::Manager *manager)
Definition: PD_HGtest.cpp:254
profile
png_infop png_charpp int png_charpp profile
Definition: png.h:2380
WAIST_FILE
#define WAIST_FILE
Definition: PD_HGtest.cpp:26
PD_HGtest_spec
static const char * PD_HGtest_spec[]
Definition: PD_HGtest.cpp:35
PD_HGtest::m_torque1Out
OutPort< TimedDoubleSeq > m_torque1Out
Definition: PD_HGtest.h:117
PD_HGtest::m_torque0
TimedDoubleSeq m_torque0
Definition: PD_HGtest.h:114
PD_HGtest::PD_HGtest
PD_HGtest(RTC::Manager *manager)
Definition: PD_HGtest.cpp:53
DLL_EXPORT
#define DLL_EXPORT
Definition: bush_customizer.cpp:20
PD_HGtest::onDeactivated
virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id)
Definition: PD_HGtest.cpp:146
PD_HGtest::m_root_trans
TimedPose3D m_root_trans
Definition: PD_HGtest.h:119
PD_HGtest::closeFiles
void closeFiles()
Definition: PD_HGtest.cpp:242
PD_HGtest::m_root_acc
TimedDoubleSeq m_root_acc
Definition: PD_HGtest.h:123
PD_HGtest::onInitialize
virtual RTC::ReturnCode_t onInitialize()
Definition: PD_HGtest.cpp:86
PD_HGtest::m_root_accOut
OutPort< TimedDoubleSeq > m_root_accOut
Definition: PD_HGtest.h:124
PD_HGtest::waist
std::ifstream waist
Definition: PD_HGtest.h:145
RTC
Definition: SimulationExecutionContext.cpp:7
PD_HGtest::m_root_transOut
OutPort< TimedPose3D > m_root_transOut
Definition: PD_HGtest.h:120
PD_HGtest::m_torque1
TimedDoubleSeq m_torque1
Definition: PD_HGtest.h:116


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:04