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


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